Accurate Distance Tracking for Optimization-based Whole-Body Humanoid Motion Planning

This paper presents a method to evaluate a distance function in semi-infinite programming (SIP) framework for optimization-based, whole-body humanoid motion planning. In our approach, the distance between pairs of robot’s link and obstacles is evaluated along the joint’s trajectory within successive time intervals. In these intervals, we compute a minimum of the continuous distance (MCD) to avoid collision. This distance is fed into the SIP solver, which subsequently suggests a new trajectory. This process is iterated until no negative distance is found anywhere in the links of the robot. We use adaptive subdivision (AS) to compute the upper and lower bounds of the MCD. We have implemented our distance evaluation methods, and have experimentally validated the proposed method to effectively and accurately find the MCDs to generate a collision-free motion for the HRP-2 humanoid robot.